import csv
import ast
import numpy as np
import matplotlib.pyplot as plt

# 读取左右脚高度
left_ankle_hights = []
right_ankle_hights = []

with open("extras_log.csv", "r") as f:
    reader = csv.reader(f)
    for row in reader:
        left_ankle_hight_list = ast.literal_eval(row[6])   # 假设左脚高度在第6列
        right_ankle_hight_list = ast.literal_eval(row[7])  # 假设右脚高度在第7列
        left_ankle_hights.append(left_ankle_hight_list)
        right_ankle_hights.append(right_ankle_hight_list)

# 转为 numpy 数组
left_ankle_hights = np.array(left_ankle_hights)   # shape: (num_rows, num_joints)
right_ankle_hights = np.array(right_ankle_hights)

# 如果是多关节，取第一个机器人的左右腿高度
# 假设第一个机器人的左腿关节索引为 0，右腿关节索引为 0
left_first_robot = left_ankle_hights[1000:1500, 0]
right_first_robot = right_ankle_hights[1000:1500, 0]

# 画图
plt.figure(figsize=(10,5))
plt.plot(left_first_robot, label="Left Ankle Height")
plt.plot(right_first_robot, label="Right Ankle Height")
plt.xlabel("Time Step")
plt.ylabel("Height (m)")
plt.title("First Robot's Ankle Heights")
plt.legend()
plt.grid(True)
plt.show()
